#include "continuousmotor.h"
#include <QDebug>

ContinuousMotor::ContinuousMotor(AbstractSystem *a_sys, int a_id, int freq, double a_delay, double a_tau, double a_imprecision):
    AbstractMotor(1000/freq), sys(a_sys), id(a_id), tau(a_tau), imprecision(a_imprecision)
{
    dt = 1./freq;
    delay = static_cast<int>(a_delay*freq);
    state = 0;
    currentCons = 0;

    timer->start();
    //AbstractMotor::exec();
}

void ContinuousMotor::update()
{
    for(int i=0; i<consigns.size(); i++)
         consigns[i].countdown--;
    if(consigns.size() > 0 && consigns[0].countdown < 0)
    {
        currentCons = consigns[0].cons;
        consigns.pop_front();
    }

    if(tau == 0)
    {
        speed = currentCons;
    }
    else
    {
        speed += ( dt*(currentCons - speed) ) / tau;
    }

    //TODO gaussian noise on result

    state += speed * dt;

    sys->checkValue(id);
}

void ContinuousMotor::setConsign(double cons)
{
    consigns << Consign(36*(cons-0.5), delay);
}

double ContinuousMotor::getState()
{
    return(state);
}

void ContinuousMotor::setState(double s)
{
    state = s;
}


